#include "industrial_robot_control/robot_controller.h"
#include "industrial_robot_control/ethercat/master.h"
#include "industrial_robot_control/modbus/modbus_client.h"
#include "industrial_robot_control/kinematics/robot_arm.h"
#include "industrial_robot_control/safety/safety_monitor.h"
#include "industrial_robot_control/visualization/web_server.h"
#include "industrial_robot_control/utils/log.h"

namespace IndustrialRobotControl {

RobotController::RobotController(const YAML::Node& config) 
    : config_(config), is_initialized_(false) {
    // 初始化各组件
    ethercat_master_ = std::make_unique<Ethercat::Master>();
    modbus_client_ = std::make_unique<Modbus::ModbusClient>();
    robot_arm_ = std::make_unique<Kinematics::RobotArm>();
    safety_monitor_ = std::make_unique<Safety::SafetyMonitor>();
    web_server_ = std::make_unique<Visualization::WebServer>();
}

RobotController::~RobotController() {
    shutdown();
}

bool RobotController::initialize() {
    LOG_INFO("Initializing robot controller");
    
    // 初始化EtherCAT主站
    if (!ethercat_master_->initialize(config_["ethercat"])) {
        LOG_ERROR("Failed to initialize EtherCAT master");
        return false;
    }
    
    // 初始化Modbus客户端
    if (config_["modbus"]["enabled"].as<bool>(false)) {
        if (!modbus_client_->connect(
            config_["modbus"]["ip"].as<std::string>(),
            config_["modbus"]["port"].as<int>())) {
            LOG_WARN("Modbus disabled - could not connect to server");
        } else {
            LOG_INFO("Modbus client connected");
        }
    }
    
    // 初始化机器人臂运动学
    if (!robot_arm_->initialize(config_["kinematics"])) {
        LOG_ERROR("Failed to initialize robot arm kinematics");
        return false;
    }
    
    // 初始化安全监控器
    if (!safety_monitor_->initialize(config_["safety"])) {
        LOG_ERROR("Failed to initialize safety monitor");
        return false;
    }
    
    // 初始化Web服务器
    if (config_["web_server"]["enabled"].as<bool>(false)) {
        if (!web_server_->start(config_["web_server"]["port"].as<int>())) {
            LOG_WARN("Web server disabled - could not start");
        } else {
            LOG_INFO("Web server started");
        }
    }
    
    is_initialized_ = true;
    LOG_INFO("Robot controller initialized successfully");
    return true;
}

void RobotController::shutdown() {
    if (is_initialized_) {
        LOG_INFO("Shutting down robot controller");
        
        // 停止Web服务器
        web_server_->stop();
        
        // 断开Modbus连接
        modbus_client_->disconnect();
        
        // 关闭EtherCAT
        ethercat_master_->shutdown();
        
        is_initialized_ = false;
    }
}

void RobotController::update() {
    // 检查安全状态
    if (!safety_monitor_->is_safe()) {
        // 安全状态异常 - 停止所有运动
        ethercat_master_->stop_all_axes();
        LOG_WARN("Safety system triggered - stopping all motion");
        return;
    }
    
    // 读取EtherCAT从站状态
    ethercat_master_->read_states();
    
    // 执行运动控制算法
    update_motion_control();
    
    // 写入控制命令到EtherCAT从站
    ethercat_master_->write_commands();
    
    // 通过Modbus读取外围设备数据
    if (modbus_client_->is_connected()) {
        read_modbus_data();
    }
    
    // 更新Web服务器数据
    if (web_server_->is_running()) {
        update_web_data();
    }
}

void RobotController::update_motion_control() {
    // 获取当前关节位置
    auto current_joints = ethercat_master_->get_joint_positions();
    
    // 计算逆运动学或生成轨迹（示例）
    // ...
    
    // 设置目标位置到EtherCAT
    // ethercat_master_->set_joint_targets(target_joints);
}

void RobotController::read_modbus_data() {
    // 读取外部传感器数据（示例）
    // auto sensor_data = modbus_client_->read_input_registers(0, 10);
    // ...
}

void RobotController::update_web_data() {
    // 更新Web服务器的实时数据
    // web_server_->update_data({
    //     {"joint_positions", ethercat_master_->get_joint_positions()},
    //     {"safety_status", safety_monitor_->get_status()}
    // });
}

} // namespace IndustrialRobotControl
